#include "fgpg_fft_ip_core_400MHz.h"

/*****************************************************************************
* Function: fgpg_fft_ip_core_400MHz()
* Purpose:  This function performs FFT/IFFT use 2/4 Sub FFT/IFFT
* Relation:
Calls       : ....
* Params:
*   <Name>          <Type>      <In/Out>    <Description>
    Coef_Re          cmplx_frN      In       output data
    Coef_Im          cmplx_frN      out      output data
    Idx              int16          In       Index to search FFT Ceof


* Return:  void
* Note:    <no>
/******************************************************************************
*  HISTORY OF CHANGES
*   <Date>          <Author>        <Version>       <DESCRIPTION>
*   2022-10-13      shanshan          V1.0.0            Created
******************************************************************************/
#define MaxSubN 2048
int fgpg_fft_ip_core_400MHz(cmplx_frN *x_in, //输入
	cmplx_frN *x_out, //输出
	int direction, //Transform direction: 1=forward FFT, 0=inverse FFT (IFFT)
	int Flag235, //  0 - 四路FFT/IFFT, 1-四路Hybrid_DFT，2 - 两路FFT/IFFT， 3 - 两路Hybrid_DFT
	int N_FFT, // FFT点数
	int radix_array[11],  //IN control parameter
	int stagenum, //IN total stage num
	char shift_factor[11],
	int order_table[3240],
	int *scaling_sch,//DynShiftFlag为固定移位时每一级的移位值，注意基4和基2的移位值不一样
	char CaseNumPath[256],
	int C_INPUT_WIDTH, //Input data width(bits)：Fix 8-34，Float 32
	int C_TWIDDLE_WIDTH  //Phase factor width (bits): Fix 8-34, Float 24-25
)
{
	int i, FFT_blk_exp = 0, shift = 1, FFT_Shift = 0;
	int N_FFT_Sub, N_Sub = 0;
	uint8 FFTCoef_Idx = 2; // used for choosing FFT table
	int delta_vs_4096 = 4096 / N_FFT;

#if UE_TX_PRINTFLAG || tft_tv_print_en
	char filename[256];
#endif

	Set_bit_width(C_INPUT_WIDTH);

	cmplx_frN *pcsRxDataFFTInPath0 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));
	cmplx_frN *pcsRxDataFFTInPath1 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));
	cmplx_frN *pcsRxDataFFTInPath2 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));
	cmplx_frN *pcsRxDataFFTInPath3 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));

	cmplx_frN *pcsRxDataFFTOutPath0 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));
	cmplx_frN *pcsRxDataFFTOutPath1 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));
	cmplx_frN *pcsRxDataFFTOutPath2 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));
	cmplx_frN *pcsRxDataFFTOutPath3 = (cmplx_frN*)malloc(MaxSubN * sizeof(cmplx_frN));

	cmplx_frM  Rotate32Path1_M[MaxSubN];
	cmplx_frM  Rotate32Path2_M[MaxSubN];
	cmplx_frM  Rotate32Path3_M[MaxSubN];

	cmplx_frN  Rotate32Path1Temp, Rotate32Path1[MaxSubN];
	cmplx_frN  Rotate32Path2Temp, Rotate32Path2[MaxSubN];
	cmplx_frN  Rotate32Path3Temp, Rotate32Path3[MaxSubN];

	if (Flag235 > 1) // 分两路
	{
		N_FFT_Sub = N_FFT / 2;
		N_Sub = 2;
		shift = 1; // 两路累加后右移1bit
		for (i = 0; i < N_FFT_Sub; i++)
		{
			pcsRxDataFFTInPath0[i].re = x_in[2 * i].re;
			pcsRxDataFFTInPath0[i].im = x_in[2 * i].im;
			pcsRxDataFFTInPath1[i].re = x_in[2 * i + 1].re;
			pcsRxDataFFTInPath1[i].im = x_in[2 * i + 1].im;
		}
	}
	else // 分四路
	{
		N_FFT_Sub = N_FFT / 4;
		N_Sub = 4;
		shift = 1; // 四路累加后右移1bit
		for (i = 0; i < N_FFT_Sub; i++)
		{
			pcsRxDataFFTInPath0[i].re = x_in[4 * i].re;
			pcsRxDataFFTInPath0[i].im = x_in[4 * i].im;
			pcsRxDataFFTInPath1[i].re = x_in[4 * i + 1].re;
			pcsRxDataFFTInPath1[i].im = x_in[4 * i + 1].im;
			pcsRxDataFFTInPath2[i].re = x_in[4 * i + 2].re;
			pcsRxDataFFTInPath2[i].im = x_in[4 * i + 2].im;
			pcsRxDataFFTInPath3[i].re = x_in[4 * i + 3].re;
			pcsRxDataFFTInPath3[i].im = x_in[4 * i + 3].im;
		}
	}

	if ((Flag235 % 2) == 0) // 基2FFT/IFFT
	{
		FFT_Shift = 0;
		FFT_blk_exp = shift;
		for (i = 0; i < log2(N_FFT / N_Sub); i++)
		{
			FFT_Shift = int(FFT_Shift + scaling_sch[i] * pow(2, i));
			FFT_blk_exp = FFT_blk_exp + scaling_sch[i];
		}

		FFT_2n_1536_fix_SDR(pcsRxDataFFTInPath0, N_FFT_Sub, FFT_Shift, FFTCoef_Idx, direction);
		memcpy(pcsRxDataFFTOutPath0, pcsRxDataFFTInPath0, N_FFT_Sub * sizeof(cmplx_frN));
		FFT_2n_1536_fix_SDR(pcsRxDataFFTInPath1, N_FFT_Sub, FFT_Shift, FFTCoef_Idx, direction);
		memcpy(pcsRxDataFFTOutPath1, pcsRxDataFFTInPath1, N_FFT_Sub * sizeof(cmplx_frN));
		if (Flag235 == 0) // 四路FFT
		{
			FFT_2n_1536_fix_SDR(pcsRxDataFFTInPath2, N_FFT_Sub, FFT_Shift, FFTCoef_Idx, direction);
			memcpy(pcsRxDataFFTOutPath2, pcsRxDataFFTInPath2, N_FFT_Sub * sizeof(cmplx_frN));
			FFT_2n_1536_fix_SDR(pcsRxDataFFTInPath3, N_FFT_Sub, FFT_Shift, FFTCoef_Idx, direction);
			memcpy(pcsRxDataFFTOutPath3, pcsRxDataFFTInPath3, N_FFT_Sub * sizeof(cmplx_frN));
		}
	}
	else // 基235
	{
		Hybrid_DFT(N_FFT_Sub,//number of PUSCH SC
			radix_array,
			stagenum,
			shift_factor,//
			order_table,
			pcsRxDataFFTInPath0,
			0
		);
		memcpy(pcsRxDataFFTOutPath0, pcsRxDataFFTInPath0, N_FFT_Sub * sizeof(cmplx_frN));

		Hybrid_DFT(N_FFT_Sub,//number of PUSCH SC
			radix_array,
			stagenum,
			shift_factor,//
			order_table,
			pcsRxDataFFTInPath1,
			1
		);
		memcpy(pcsRxDataFFTOutPath1, pcsRxDataFFTInPath1, N_FFT_Sub * sizeof(cmplx_frN));

		if (Flag235 == 1) // 四路DFT
		{
			Hybrid_DFT(N_FFT_Sub,//number of PUSCH SC
				radix_array,
				stagenum,
				shift_factor,//
				order_table,
				pcsRxDataFFTInPath2,
				2
			);
			memcpy(pcsRxDataFFTOutPath2, pcsRxDataFFTInPath2, N_FFT_Sub * sizeof(cmplx_frN));

			Hybrid_DFT(N_FFT_Sub,//number of PUSCH SC
				radix_array,
				stagenum,
				shift_factor,//
				order_table,
				pcsRxDataFFTInPath3,
				3
			);
			memcpy(pcsRxDataFFTOutPath3, pcsRxDataFFTInPath3, N_FFT_Sub * sizeof(cmplx_frN));
		}
	}

#if UE_TX_PRINTFLAG || tft_tv_print_en
	sprintf_s(filename, "%s/FFT%dOutPath0.dat", CaseNumPath, N_FFT_Sub);
	data_print_cfrN_400MHz(filename, pcsRxDataFFTOutPath0, N_FFT_Sub, C_INPUT_WIDTH);
	sprintf_s(filename, "%s/FFT%dOutPath1.dat", CaseNumPath, N_FFT_Sub);
	data_print_cfrN_400MHz(filename, pcsRxDataFFTOutPath1, N_FFT_Sub, C_INPUT_WIDTH);
	if (Flag235 < 2)
	{
		sprintf_s(filename, "%s/FFT%dOutPath2.dat", CaseNumPath, N_FFT_Sub);
		data_print_cfrN_400MHz(filename, pcsRxDataFFTOutPath2, N_FFT_Sub, C_INPUT_WIDTH);
		sprintf_s(filename, "%s/FFT%dOutPath3.dat", CaseNumPath, N_FFT_Sub);
		data_print_cfrN_400MHz(filename, pcsRxDataFFTOutPath3, N_FFT_Sub, C_INPUT_WIDTH);
	}
#endif

	// 旋转因子生成
	cmplx_frN RotateFactorPath1[1024], RotateFactorPath2[1024], RotateFactorPath3[1024];

	if ((Flag235 % 2) == 1) // 基235， exp（-j*coef*2*pi/N）
	{
		twiddle_calc(N_FFT, 1, RotateFactorPath1, C_TWIDDLE_WIDTH, N_Sub);

		if (Flag235 == 1) // 四路基235
		{
			twiddle_calc(N_FFT, 2, RotateFactorPath2, C_TWIDDLE_WIDTH, N_Sub);
			twiddle_calc(N_FFT, 3, RotateFactorPath3, C_TWIDDLE_WIDTH, N_Sub);
		}
	}

	for (i = 0; i < N_FFT_Sub; i++)
	{
		// 旋转因子生成
		if ((Flag235 % 2) == 0) // 4096点FFT
		{
			// exp(-j*2*pi*n/N)
			FFT_Coef_LookUp2(&RotateFactorPath1[i].re,
				&RotateFactorPath1[i].im,
				i * delta_vs_4096, //Index to search FFT Ceof
				C_TWIDDLE_WIDTH  //比特长度
			);
			if (Flag235 < 2)
			{
				FFT_Coef_LookUp2(&RotateFactorPath2[i].re,
					&RotateFactorPath2[i].im,
					2 * i * delta_vs_4096, //Index to search FFT Ceof
					C_TWIDDLE_WIDTH  //比特长度
				);
				FFT_Coef_LookUp2(&RotateFactorPath3[i].re,
					&RotateFactorPath3[i].im,
					3 * i * delta_vs_4096, //Index to search FFT Ceof
					C_TWIDDLE_WIDTH  //比特长度
				);
			}

			// exp(j*2*pi*n/N)
			if (direction == 0) // IFFT
			{
				RotateFactorPath1[i].im = negate_fr1xN(RotateFactorPath1[i].im);
				if (Flag235 < 2)
				{
					RotateFactorPath2[i].im = negate_fr1xN(RotateFactorPath2[i].im);
					RotateFactorPath3[i].im = negate_fr1xN(RotateFactorPath3[i].im);
				}
			}
		}

		// 三路数据乘以旋转因子
		Rotate32Path1_M[i] = cmlt_frM(pcsRxDataFFTOutPath1[i], RotateFactorPath1[i]);
		if (Flag235 < 2)
		{
			Rotate32Path2_M[i] = cmlt_frM(pcsRxDataFFTOutPath2[i], RotateFactorPath2[i]);
			Rotate32Path3_M[i] = cmlt_frM(pcsRxDataFFTOutPath3[i], RotateFactorPath3[i]);
		}

		Rotate32Path1[i].re = (fractN)(Rotate32Path1_M[i].re >> (C_INPUT_WIDTH));
		Rotate32Path1[i].im = (fractN)(Rotate32Path1_M[i].im >> (C_INPUT_WIDTH));
		if (Flag235 < 2)
		{
			Rotate32Path2[i].re = (fractN)(Rotate32Path2_M[i].re >> (C_INPUT_WIDTH));
			Rotate32Path2[i].im = (fractN)(Rotate32Path2_M[i].im >> (C_INPUT_WIDTH));
			Rotate32Path3[i].re = (fractN)(Rotate32Path3_M[i].re >> (C_INPUT_WIDTH));
			Rotate32Path3[i].im = (fractN)(Rotate32Path3_M[i].im >> (C_INPUT_WIDTH));
		}

		if (Flag235 < 2) // 分四路
		{
			//  第一路[1 1 1 1]
			x_out[i] = cfrNSum_cfrM(pcsRxDataFFTOutPath0[i], Rotate32Path1[i], Rotate32Path2[i], Rotate32Path3[i], shift);

			//	第二路[1 - j - 1 j]
			if (direction == 1) // FFT,[1 - j - 1 j]
			{
				Rotate32Path1Temp = compose_cfrN(Rotate32Path1[i].im, -Rotate32Path1[i].re); // -j(a + bj) = b - aj;
				Rotate32Path3Temp = compose_cfrN(-Rotate32Path3[i].im, Rotate32Path3[i].re); // j(a + bj) = -b + aj;
			}
			else // IFFT,[1 j - 1 -j]
			{
				Rotate32Path1Temp = compose_cfrN(-Rotate32Path1[i].im, Rotate32Path1[i].re); // j(a + bj) = -b + aj;
				Rotate32Path3Temp = compose_cfrN(Rotate32Path3[i].im, -Rotate32Path3[i].re); // -j(a + bj) = b - aj;
			}
			Rotate32Path2Temp = compose_cfrN(-Rotate32Path2[i].re, -Rotate32Path2[i].im); // -(a + bj) = -a - bj;
			x_out[i + N_FFT_Sub] = cfrNSum_cfrM(pcsRxDataFFTOutPath0[i], Rotate32Path1Temp, Rotate32Path2Temp, Rotate32Path3Temp, shift);

			//	第三路[1 - 1 1 - 1]
			Rotate32Path1Temp = compose_cfrN(-Rotate32Path1[i].re, -Rotate32Path1[i].im); // -(a + bj) = -a - bj;
			Rotate32Path3Temp = compose_cfrN(-Rotate32Path3[i].re, -Rotate32Path3[i].im); // -(a + bj) = -a - bj;
			x_out[i + N_FFT_Sub * 2] = cfrNSum_cfrM(pcsRxDataFFTOutPath0[i], Rotate32Path1Temp, Rotate32Path2[i], Rotate32Path3Temp, shift);

			//	第四路[1 j - 1 - j]
			if (direction == 1) // FFT,[1 j - 1 - j]
			{
				Rotate32Path1Temp = compose_cfrN(-Rotate32Path1[i].im, Rotate32Path1[i].re); // j(a + bj) = -b + aj;
				Rotate32Path3Temp = compose_cfrN(Rotate32Path3[i].im, -Rotate32Path3[i].re); // -j(a + bj) = b - aj;
			}
			else // IFFT,[1 -j - 1 j]
			{
				Rotate32Path1Temp = compose_cfrN(Rotate32Path1[i].im, -Rotate32Path1[i].re); // -j(a + bj) = b - aj;
				Rotate32Path3Temp = compose_cfrN(-Rotate32Path3[i].im, Rotate32Path3[i].re); // j(a + bj) = -b + aj;
			}

			Rotate32Path2Temp = compose_cfrN(-Rotate32Path2[i].re, -Rotate32Path2[i].im); // -(a + bj) = -a - bj;
			x_out[i + N_FFT_Sub * 3] = cfrNSum_cfrM(pcsRxDataFFTOutPath0[i], Rotate32Path1Temp, Rotate32Path2Temp, Rotate32Path3Temp, shift);
		}
		else // 分两路
		{
			//  第一路[1 1]
			x_out[i] = cfrNSum_cfrM_2path(pcsRxDataFFTOutPath0[i], Rotate32Path1[i], shift, 1); // add

			//  第二路[1 -1]
			x_out[i + N_FFT_Sub] = cfrNSum_cfrM_2path(pcsRxDataFFTOutPath0[i], Rotate32Path1[i], shift, 0); // sub
		}
	}

#if 1 == UE_TX_PRINTFLAG || tft_tv_print_en
	sprintf_s(filename, "%s/SubFFT_Multi_RotateFactor%dPath1.dat", CaseNumPath, N_FFT_Sub);
	data_print_cfrN_400MHz(filename, Rotate32Path1, N_FFT_Sub, C_INPUT_WIDTH);
	if (Flag235 < 2)
	{
		sprintf_s(filename, "%s/SubFFT_Multi_RotateFactor%dPath2.dat", CaseNumPath, N_FFT_Sub);
		data_print_cfrN_400MHz(filename, Rotate32Path2, N_FFT_Sub, C_INPUT_WIDTH);
		sprintf_s(filename, "%s/SubFFT_Multi_RotateFactor%dPath3.dat", CaseNumPath, N_FFT_Sub);
		data_print_cfrN_400MHz(filename, Rotate32Path3, N_FFT_Sub, C_INPUT_WIDTH);
	}

	sprintf_s(filename, "%s/RotateFactor%dPath1.dat", CaseNumPath, N_FFT_Sub);
	data_print_cfrN_400MHz(filename, RotateFactorPath1, N_FFT_Sub, C_INPUT_WIDTH);
	if (Flag235 < 2)
	{
		sprintf_s(filename, "%s/RotateFactor%dPath2.dat", CaseNumPath, N_FFT_Sub);
		data_print_cfrN_400MHz(filename, RotateFactorPath2, N_FFT_Sub, C_INPUT_WIDTH);
		sprintf_s(filename, "%s/RotateFactor%dPath3.dat", CaseNumPath, N_FFT_Sub);
		data_print_cfrN_400MHz(filename, RotateFactorPath3, N_FFT_Sub, C_INPUT_WIDTH);
	}
#endif

	free(pcsRxDataFFTInPath0);
	free(pcsRxDataFFTInPath1);
	free(pcsRxDataFFTInPath2);
	free(pcsRxDataFFTInPath3);
	free(pcsRxDataFFTOutPath0);
	free(pcsRxDataFFTOutPath1);
	free(pcsRxDataFFTOutPath2);
	free(pcsRxDataFFTOutPath3);

	return FFT_blk_exp;
}
cmplx_frN cfrNSum_cfrM(cmplx_frN add0, cmplx_frN add1, cmplx_frN add2, cmplx_frN add3, int shift)
{
	cmplx_frM add_cfrM0 = compose_cfrM(add0.re, add0.im);
	cmplx_frM add_cfrM1 = compose_cfrM(add1.re, add1.im);
	cmplx_frM add_cfrM2 = compose_cfrM(add2.re, add2.im);
	cmplx_frM add_cfrM3 = compose_cfrM(add3.re, add3.im);
	cmplx_frM sum_cfrM;
	cmplx_frN sum_cfrN;

	sum_cfrM = cadd_frM(cadd_frM(add_cfrM0, add_cfrM1), cadd_frM(add_cfrM2, add_cfrM3)); // sum
	sum_cfrN.re = (fractN)(sum_cfrM.re >> shift);
	sum_cfrN.im = (fractN)(sum_cfrM.im >> shift);
	return sum_cfrN;
}

cmplx_frN cfrNSum_cfrM_2path(cmplx_frN add0, cmplx_frN add1, int shift, int Add_Flag)
{
	cmplx_frM add_cfrM0 = compose_cfrM(add0.re, add0.im);
	cmplx_frM add_cfrM1 = compose_cfrM(add1.re, add1.im);
	cmplx_frM sum_cfrM;
	cmplx_frN sum_cfrN;

	if (Add_Flag == 1) // add
	{
		sum_cfrM = cadd_frM(add_cfrM0, add_cfrM1);
	}
	else //sub
	{
		sum_cfrM = csub_frM(add_cfrM0, add_cfrM1);
	}

	sum_cfrN.re = (fractN)(sum_cfrM.re >> shift);
	sum_cfrN.im = (fractN)(sum_cfrM.im >> shift);
	return sum_cfrN;
}
void twiddle_calc(int N_DFT, int coef, cmplx_frN twiddle_readin[1024], int C_INPUT_WIDTH, int N_Sub)
{
	int j, CoefIdx;
	int64 theta_basic;
	double pi = 3.14159265358979;

	Set_bit_width(C_INPUT_WIDTH);

	theta_basic = (int64)(1.0 / N_DFT * pow(2.0, 32 - 1));

	for (j = 0; j < N_DFT / N_Sub; j++)
	{
		CoefIdx = (j * theta_basic * coef >> (31 - 13));
		exp_1jx2xpixnvs8192_fxptable(&twiddle_readin[j], CoefIdx, 1, C_INPUT_WIDTH);
	}
}

void data_print_cfrN_400MHz(char *d_name, cmplx_frN *data, int len, int bitlen)
{
	int j;
	FILE *Fid;

	Fid = fopen(d_name, "a");
	if (bitlen == 16)
	{
		for (j = 0; j < len; j++)
		{
			fprintf(Fid, "%04x%04x\n", (uint16)data[j].im & 0xffff, (uint16)data[j].re & 0xffff);
		}
	}
	else
	{
		for (j = 0; j < len; j++)
		{
			if (data[j].re >= 0 && data[j].im >= 0)
			{
				fprintf(Fid, "%07x\n", ((data[j].im) << bitlen) + (uint32)(data[j].re));
			}
			else if (data[j].re >= 0 && data[j].im < 0)
			{
				fprintf(Fid, "%07x\n", (((1 << bitlen) + data[j].im) << bitlen) + (uint32)(data[j].re));
			}
			else if (data[j].re < 0 && data[j].im < 0)
			{
				fprintf(Fid, "%07x\n", (((1 << bitlen) + data[j].im) << bitlen) + (uint32)((1 << bitlen) + data[j].re));
			}
			else
			{
				fprintf(Fid, "%07x\n", ((1 << bitlen) + data[j].re) + (uint32)(data[j].im << bitlen));
			}
		}
	}
	fclose(Fid);
}